/*
 * htd-45.c
 *
 *  Created on: Jun 9, 2025
 *      Author: yangqiuqiang
 */

#include "htd-45.h"
#include "uart_n.h"
char LobotCheckSum(char buf[])
{
  int i;
  char check;
  uint16_t temp = 0;
  for (i = 2; i < buf[3] + 2; i++) {
    temp += buf[i];
  }
  temp = ~temp;
  check = (char)temp;
  return check;
}

void LobotSerialServoReadTempLimit(char id)
{
  //55 55 01 03 19 E2
  char buf[6]={0x55,0x55,id,0x03,0x19,0x00};
  buf[5]= LobotCheckSum(buf);
  int buf_n=sizeof(buf)/sizeof(char);
  USART2_SendString(buf_n,buf);
}
void LobotSerialServoReadVinLimit(char id)
{
  //55 55 01 03 19 E2
  char buf[6]={0x55,0x55,id,0x03,0x17,0x00};
  buf[5]= LobotCheckSum(buf);
  int buf_n=sizeof(buf)/sizeof(char);
  USART2_SendString(buf_n,buf);
}

void LobotSerialServoReadPosition(char id)
{
    char buf[6]={0x55,0x55,id,0x03,0x1C,0x00};
    buf[5]= LobotCheckSum(buf);
    int buf_n=sizeof(buf)/sizeof(char);
    USART2_SendString(buf_n,buf);
}

void LobotSerialServoReadVin(char id)
{
    char buf[6]={0x55,0x55,id,0x03,0x1B,0x00};
    buf[5]= LobotCheckSum(buf);
    int buf_n=sizeof(buf)/sizeof(char);
    USART2_SendString(buf_n,buf);
}
void LobotSerialServoReadTemp(char id)
{
    char buf[6]={0x55,0x55,id,0x03,0x1A,0x00};
    buf[5]= LobotCheckSum(buf);
    int buf_n=sizeof(buf)/sizeof(char);
    USART2_SendString(buf_n,buf);
}
void LobotSerialServoLoad(char id)
{
    char buf[7]={0x55,0x55,id,0x04,0x1F,0x01,0x00};
    buf[6]= LobotCheckSum(buf);
    int buf_n=sizeof(buf)/sizeof(char);
    USART2_SendString(buf_n,buf);

}

void LobotSerialServoUnload(char id)
{
    char buf[7]={0x55,0x55,id,0x04,0x1F,0x00,0x00};
    buf[6]= LobotCheckSum(buf);
    int buf_n=sizeof(buf)/sizeof(char);
    USART2_SendString(buf_n,buf);
}

void LobotSerialServoMove(uint8_t id, int16_t position, uint16_t time)
{
  char buf[10];
  if(position < 0)
    position = 0;
  if(position > 1000)
    position = 1000;
  buf[0] = buf[1] = LOBOT_SERVO_FRAME_HEADER;
  buf[2] = id;
  buf[3] = 7;
  buf[4] = LOBOT_SERVO_MOVE_TIME_WRITE;
  buf[5] = GET_LOW_BYTE(position);
  buf[6] = GET_HIGH_BYTE(position);
  buf[7] = GET_LOW_BYTE(time);
  buf[8] = GET_HIGH_BYTE(time);
  buf[9] = LobotCheckSum(buf);
  USART2_SendString(10,buf);
}

